#include <Wire.h>


//this one has been modified so that 1 interreupt pin per can controller is used for rx interreupts instead of 4 pins
#include "can_constants.h"
#include "spiCAN.h"
#define RTS0PIN_1 7
#define RTS1PIN_1 6
#define RTS2PIN_1 5
//#define RX0BF 4
//#define RX1BF 3
#define C1INT 0
#define C2INT 1

#define SS1_PIN   10
#define SS2_PIN   9
#define RTS0PIN_2 14
#define RTS1PIN_2 15
#define RTS2PIN_2 16
//#define RX0BF2 17
//#define RX1BF2 12

#include "WProgram.h"
void setup();
void loop();
Datapack checkmsg(Datapack a);
void cantext(char text[], spiCAN canNode);
void loadtext(byte cantext[], char outtext[],boolean whichcan);
void togglePinLow(int Pin);
void can1int();
void can2int();
void receiveEvent(int howMany);
Datapack g;
boolean bypassmsg;
int r_adr;
byte dlc;
boolean intercept =false;
char disptext[14] = "Blah";
byte k=0;
byte val=0;
byte dummy;
boolean msginCAN1=false;
boolean msginCAN2=false;
spiCAN spiCAN1(SS1_PIN,RTS0PIN_1,RTS1PIN_1,RTS2PIN_1); //instanciate
spiCAN spiCAN2(SS2_PIN,RTS0PIN_2,RTS1PIN_2,RTS2PIN_2); //instanciate


void setup(){

  //  pinMode(RX0BF,INPUT);
  //  pinMode(RX1BF,INPUT);
  //  pinMode(RX0BF2,INPUT);
  //  pinMode(RX1BF2,INPUT);
  // Serial.println("Reseting CAN");

  Serial.begin(56700);
  Wire.begin(0x09);
  Wire.onReceive(receiveEvent);
  spiCAN1.initController();
  spiCAN2.initController();
  bypassmsg = false; //we'll use this to determine the state of whether a volume msg was being sent

  //Serial.println("Setting to Normal Mode");
  //Serial.println("Reading CAN Status");
  //Serial.println("Send message in buffer");
  pinMode(19,INPUT);
  digitalWrite(19,false);
  //

  attachInterrupt(C1INT,can1int, FALLING);
  attachInterrupt(C2INT,can2int, FALLING);
}


void loop(){

  while(Serial.available()>0&&k<12){
    val = Serial.read();
    Serial.print(val);
    if(val=='x'&&k==0){ 
      Serial.println("intercept command"); 
      intercept = true;
      disptext[0]='\0';
    }

    else if(val=='s'&&k==0){
      intercept = false;
      Serial.println("stop intercept command"); 
      Serial.flush();
    }
    else if(k==0&&val!='s'){
      Serial.println("command misunderstood");
      Serial.flush(); //if first character wasn't x or s  then flush buffer

    }
    if (k>0){
      // Serial.println("yo");
      disptext[k-1]=val;
      if(Serial.available()==0){
        disptext[k]='\0';
      }
    }
    k++;
  }
  k=0;

  //  if(digitalRead(19)==true){
  //    intercept=true;
  //    Serial.println("meas true");
  //  }
  //  else{
  //    intercept = false;
  //    Serial.println("meas false");
  //  }

  if(msginCAN1){
    msginCAN1=false;  //reset this flag
    //    Serial.println("MSG Buffer 1 0");
    dummy = spiCAN1.readRegister(CANINTF);  //let's check which buffer(s) are flagged
    if(bitRead(dummy,0)){
      g= spiCAN1.readRXBuf(0);
      g = checkmsg(g);
      spiCAN2.sendCANMessage(g);
    };
    if(bitRead(dummy,1)){
      g= spiCAN1.readRXBuf(1);
      g = checkmsg(g);
      spiCAN2.sendCANMessage(g);
    }
  }

  if(msginCAN2){
    msginCAN2=false;  //reset this flag
    //    Serial.println("MSG Buffer 1 0");
    dummy = spiCAN2.readRegister(CANINTF);  //let's check which buffer(s) are flagged
    if(bitRead(dummy,0)){
      g= spiCAN2.readRXBuf(0);
      g = checkmsg(g);
      spiCAN1.sendCANMessage(g);
    };
    if(bitRead(dummy,1)){
      g= spiCAN2.readRXBuf(1);
      g = checkmsg(g);
      spiCAN1.sendCANMessage(g);
    }
  }


}
Datapack checkmsg(Datapack a){
  if (intercept==true){
    //Serial.println("intercepting");
    switch(a.sid){
    case 0x290:
      if(a.data[1]==0x20){
        if(a.data[2]==0x57&&a.data[3]==0x41&&a.data[4]==0x49&&a.data[5]==0x54&&a.data[6]==0x20&&a.data[7]==0x20){
          bypassmsg=true;
        }//WAIT
        else if(a.data[2]==0x49&&a.data[3]==0x4E&&a.data[4]==0x20&&a.data[5]==0x20&&a.data[6]==0x20){
          bypassmsg=true;
        }//IN
        else if(a.data[2]==0x4D&&a.data[3]==0x55&&a.data[4]==0x54&&a.data[5]==0x45&&a.data[6]==0x20&&a.data[7]==0x20){
          bypassmsg=true;
        }//MUTE
        if(a.data[2]==0x20){


          if(a.data[3]==0x56&&a.data[4]==0x4f&&a.data[5]==0x4c){
            bypassmsg=true; //VOL
          }
          else if(a.data[3]==0x41&&a.data[4]==0x4c&&a.data[5]==0x43){
            bypassmsg=true;
          } //ALC
          else if(a.data[3]==0x42&&a.data[4]==0x41&&a.data[5]==0x53&&a.data[6]==0x53){
            bypassmsg=true;
          } //BASS
          else if(a.data[3]==0x54&&a.data[4]==0x52&&a.data[5]==0x45&&a.data[6]==0x42){
            bypassmsg=true;
          } //TREB
          else if(a.data[3]==0x46&&a.data[4]==0x41&&a.data[5]==0x44&&a.data[6]==0x45){
            bypassmsg=true;
          }//FADE
          else if(a.data[3]==0x42&&a.data[4]==0x41&&a.data[5]==0x4c&&a.data[6]==0x20){
            bypassmsg=true;
          }//BAL 
          else if(a.data[3]==0x42&&a.data[4]==0x45&&a.data[5]==0x45&&a.data[6]==0x50){
            bypassmsg=true;
          }//BEEP
        }
      }
      else{
        loadtext(a.data,disptext,0);
      }

      return a;
      break;
    case 0x291:
      if(!bypassmsg){
        loadtext(a.data,disptext,1);

      }
      bypassmsg=false;
      return a;
      break;
    case 0x28f:  //remove extra characters if intercepting
      a.data[3]=0;
      return a;
      break;
    default:
      return a;
    }
  }
  else{
    //Serial.println("no intercepting");
    return a;
  }
}
void cantext(char text[], spiCAN canNode){
  byte text1[9] = {
    0xC0,0x20,0x20,0x20,0x20,0x20,0x20,0x20,'\0'              };   //0xC0 starts the CAN frame the next 7 bytes are data
  byte text2[9] = {
    0x87,0x20,0x20,0x20,0x20,0x20,0x20,0x20,'\0'              };   //0x87 starts the CAN frame the next 7 bytes are data
  byte i;
  for (i=1;(text[i-1]!='\0')&&(text1[i]!='\0');i++){
    text1[i] = text[i-1];
  }
  if (i>=8){ //only bother to fill 2nd byte if neccessary
    for (i=1;(text[i+6]!='\0')&&(text2[i]!='\0');i++){
      text2[i] = text[i+6];
    }
  }
  canNode.sendCANMessage(0x290,8,text1);
  canNode.sendCANMessage(0x291,8,text2);
}
void loadtext(byte cantext[], char outtext[],boolean whichcan){
  byte i; 
  for (i=1;i<8;i++){ //if it's a message to be overriden let's fill with 0x20's
    cantext[i]=0x20;
  }


  if(!whichcan){ //if whichcan=0 set 0x290 frame otherwise we set the 0x291 frame
    //cantext[0]=0xC0;
    for (i=1;(outtext[i-1]!='\0')&&(i<8);i++){
      cantext[i] = outtext[i-1];
    }
  }
  else{
    i=0;
    while(outtext[i]!='\0'&&i<7){ 
      i++;
    }
    if (i>=7){   // cantext[0]=0x87;  //make sure there first 7 characters have data before filling the back set
      for (byte i=1;(outtext[i+6]!='\0')&&(i<6);i++){ //last 2 bytes in 0x291 field are unknown so we stop at i<6
        cantext[i] = outtext[i+6];
      }
    }
  }
}
void togglePinLow(int Pin){//toggle a Pin to Low regardless of current state.
  digitalWrite(Pin,HIGH);
  digitalWrite(Pin,LOW);
}
void can1int(){
  msginCAN1=true;
}
void can2int(){
  msginCAN2=true;
}


void receiveEvent(int howMany) { 
  byte data[13];
  byte dummy;
  byte i=0;
  boolean cmdbyte = false;  //we'll use this to determine whether we are parsing within a command
  Serial.println("I2C Message Received...");

  while(0<Wire.available()&&i<12){
    data[i] = Wire.receive(); // receive byte as a character 
i++;  
}
  while(0<Wire.available()){  //if there are still bytes in the buffer we'll throw'em away
   dummy=Wire.receive(); 
   Serial.println("extra byte discarded!");
  }
  //first we'll branch according to the first byte
switch(data[0]){
case 0x01:   //intercept mode change addr byte
        Serial.println("Intercept Address Byte...");
if (i>1){
 switch(data[1]){
  case 0x00:
  intercept=false;
  Serial.println("0x00 Recevied, setting intercept false");
  break;
  case 0x01:
  intercept=true;
  Serial.println("0x00 Recevied, setting intercept true");
  break;
  default:
  Serial.println("Unrecognized mode");
  break;
 } 
}
else{Serial.println("Mode byte not received not doing anything");}
break;
case 0x02:
Serial.print("Setting Intercept Message to:");
for (byte j=0;j<(i-1);j++){  //load the data array into disptext
  disptext[j]=data[j+1];
  Serial.print(data[j+1]);
}
disptext[i-1]='\0';  //terminate the string
break;
}
}  
  
  
  



int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

